/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
#include "HoughAngleMatcher.h"
		
#define MAP_FILE_NAME "../res/autmap-small.txt"		
#include<stdio.h>
#include<stdlib.h>
#include<math.h>
		
#include "TOccupancyGrid.h" ///< Occupancy grid class
#include "glutImageGUI.h"   ///< For visualisation
#include "JPEGEncoder.hpp"  ///< For saving JPEG
		


void timer(bool isStart){
		static long int ss1,us1;
		long int ss2,us2,sec,us,ms;
		float fms;
		
		if(isStart) ownTime_GetTimeOfDay(&ss1,&us1);
		else{
				ownTime_GetTimeOfDay(&ss2,&us2);
				sec = ss2-ss1;
				us = us2-us1;
				ms = sec*1000+us/1000;
				fms=(float)sec*1000.0+(float)us/1000.0;
				fprintf(stderr,"Time elapsed (%.6fms = %.6fs) \n",fms,fms/1000.0);
		}
}

///////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////
/// Read Scan measurement file with arbitrary number of items (and allowing white space before '\n')
///////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////
int forwardString(const char *buf, int begin,int items){
		int i=begin+1;
		int cnt = 0;
		while(cnt<items){
				if(buf[i]==' ') cnt++;
				i++;
		}
		return i;
}

/// For reading the data from file
int readScanLine(FILE *f,Grid::TScan &meas, Grid::pose &p ){
		float time_s;
		int i=0;
		char in_line[16096];
		char *ret;
		char c=0;
		
		ret = fgets (in_line, 16096, f );
		if(ret==NULL){
				if(feof(f)){
						fprintf(stderr,"Reached end of file -- Alles klar!\n");
						return 0;
				}else{
						fprintf(stderr,"Some error occurred...sorry contact Jari :)\n");
						return 1;
				}
		}
		int cnt=0;
		while( (c!='\n')){
				if(in_line[i]==' ') cnt++;
				else if(in_line[i]=='\n') break;
				i++;
		}
		//fprintf(stderr,"There is %d floats in this file (= %d meas points)\n",cnt,(cnt-3)/2);
		sscanf(in_line,"%f %f %f ",&p.x,&p.y,&p.a); ///< Pose header
		int pointer_ind=0;
		pointer_ind = forwardString(in_line,0,3); ///search the next begin for float
		meas.N = (cnt-3)/2;
		i =0;
		while(i<((cnt-3)/2)){
				sscanf((in_line+pointer_ind),"%f %f",&meas.r[i],&meas.a[i]);
				pointer_ind = forwardString(in_line,pointer_ind,2); ///search the next begin for float
				i++;
		}
	
		return 0;
}



float topi(float da){
		if(da>M_PI)while(da>M_PI) da-=2.0*M_PI;
		else if(da<-M_PI)while(da<-M_PI) da+=2.0*M_PI;
		return da;
}
		//////////////////////////////////////////////////////////////////////////////////
		//////////////////////////////////////////////////////////////////////////////////
		///DRAWING STUFF
		//////////////////////////////////////////////////////////////////////////////////
		//////////////////////////////////////////////////////////////////////////////////
		void setRGBpix(unsigned char *rgb,int i,int j, int Nx, unsigned char r,unsigned char g, unsigned char b ){
				rgb[3*Nx*j+3*i]=r;
				rgb[3*Nx*j+3*i+1]=g;
				rgb[3*Nx*j+3*i+2]=b;
		}
		
		void setRGBmap(unsigned char *rgb,TOccupancyGrid *gr, unsigned char r,unsigned char g, unsigned char b){
				float *data = gr->getDataPtr();
				int Nx,Ny;
				gr->getDim(Nx,Ny);
				float v;
		
				for(int j=0;j<Ny;j++){
						for(int i=0;i<Nx;i++){
								v = data[Nx*j+i];
								if(v>0.6) v = 0; else v = 0.99; 
						
								setRGBpix(rgb,i,j, Nx, r*v,g*v,b*v);
						}
				}	
		}

void setRGBpixBold(unsigned char *rgb,int i,int j, int Nx, unsigned char r,unsigned char g, unsigned char b){
		for(int ii=i-1;ii<i+1;ii++){
				for(int jj=j-1;jj<j+1;jj++){
						setRGBpix(rgb,ii,jj,Nx,r,g,b);
				}
		}

}

void drawLine(unsigned char *RGB, int Nx,int Ny,float resolution,   ///Grid parameters
uint pix, uint piy,float angle, float length, ///< Start pixel, angle to draw and linelength
unsigned char r,unsigned char g,unsigned char b){
		float dy = sin(angle);
		float dx = cos(angle);
		float rr=0;
		int i=0;
		uint x,y;
		while(rr<length){
				x = pix + i*dx;
				y = piy - i*dy;
				setRGBpixBold(RGB,x,y, Nx, r,g,b);
				i++;
				rr+=resolution;
		}
}

void drawLineThin(unsigned char *RGB, int Nx,int Ny,float resolution,   ///Grid parameters
					uint pix, uint piy,float angle, float length, ///< Start pixel, angle to draw andlinelength
										unsigned char r,unsigned char g,unsigned char b)
{
				float dy = sin(angle);
				float dx = cos(angle);
						float rr=0;
						int i=0;
						uint x,y;
						while(rr<length){
								x = pix + i*dx;
								y = piy - i*dy;
								setRGBpix(RGB,x,y, Nx, r,g,b);
								i++;
				rr+=resolution;
		}
}

void drawCross(unsigned char *RGB, int Nx,int Ny,float resolution,   ///Grid parameters
				uint pix, uint piy, float length, ///< Start pixel, angle to draw and linelength
										unsigned char r,unsigned char g,unsigned char b)
{
		float angle = M_PI/4;
		for(int i=0;i<3;i++){
				drawLineThin(RGB, Nx,Ny,resolution, pix, piy,angle, length, r,g,b);
				angle+=M_PI/2;
		}
}


void saveJPEG(const char *fname, unsigned char *rgb,int Nx, int Ny){
		CJPEGEncoder encoder;
		encoder.SetRGBDataPtr((const char *)rgb,Nx,Ny);
		FILE *f = fopen(fname,"wb");
		fwrite(encoder.GetJPEGDataPtr(), 
						encoder.GetJPEGDataSize(), 
						1, f);
		fclose(f);

}

float randomi(){
		return (2.0 * ((float)rand()/(float)RAND_MAX) - 1.0);
}
//////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////

void saveScan(const char *fname, Grid::TScan &s){
		FILE *f = fopen(fname,"wt");
		if(f==NULL){
				fprintf(stderr,"Not good - unable to save!\n");
				exit(1);
		}

		for(int i=0;i<s.N;i++){
				fprintf(f,"%f %f\n",s.r[i],s.a[i]);
		}
		fclose(f);
}

void drawRobot(unsigned char *rgb, TOccupancyGrid *O, Grid::pose pos, unsigned char r,unsigned char g, unsigned char b ){
		Grid::TScan s;
		s.alloc(181);
		Grid::TCoord2D ocoord;
		Grid::TPixel pix;
		float range;


		int Nx,Ny;
		O->getDim(Nx,Ny);

		O->getScan(pos.x,pos.y, pos.a-M_PI/2.0, 181, M_PI/180, 0.9, s);
		for(int i=0;i<181;i++){
				if(s.r[i] >0){
						range = s.r[i] + 0.15*randomi();
						ocoord.x = pos.x +  range * cos(s.a[i]+pos.a-M_PI/2.0) ;ocoord.y = pos.y+range * sin(s.a[i]+pos.a-M_PI/2.0);
						pix = O->toPixel(ocoord);
						drawCross(rgb, Nx,Ny,O->getResolution(),pix.x,pix.y, 0.2, r,g,b);
				}
		}

		ocoord.x = pos.x ;ocoord.y = pos.y;
		pix = O->toPixel(ocoord);
		drawLine(rgb, Nx,Ny, O->getResolution(),pix.x,pix.y,pos.a, 1.0,r,g,b);
		drawLine(rgb, Nx,Ny, O->getResolution(),pix.x,pix.y,pos.a+M_PI/2, 0.5,r,g,b);
		drawLine(rgb, Nx,Ny, O->getResolution(),pix.x,pix.y,pos.a-M_PI/2, 0.5,r,g,b);


}

void TestHoughAngle(
				unsigned char *rgb, 
				TOccupancyGrid *O, 
				Grid::pose ref_pos)
{
		Grid::TScan s,m;
		s.alloc(1440);
		m.alloc(181);
		Grid::TCoord2D ocoord;
		Grid::TPixel pix;
		float range;
		srand(time(NULL));
		int Nx,Ny;
		O->getDim(Nx,Ny);
		
		///Take reference measurement
		O->getScan(ref_pos.x,ref_pos.y, ref_pos.a-M_PI/2.0, 1440, M_PI/720, 0.9, s);
		
		Grid::pose meas_pos;
		meas_pos = ref_pos;
		meas_pos.x += 0.4 * randomi();
		meas_pos.y += 0.4 * randomi();
		meas_pos.a += M_PI * randomi();
		
		///Take measurement
		O->getScan(meas_pos.x,meas_pos.y, meas_pos.a-M_PI/2.0, 181, M_PI/180, 0.9, m);
		
		CHoughAngleMatcher matcher(500,360);
		std::vector<TAngleMatch> matches;
		
		CEnvironmentMeasurement2D meas,ref;
		
		for(int i=0;i<s.N;i++){
				ref.points.push_back(TMeasurementPoint(
								(s.r[i]+randomi()*0.04)*cos(s.a[i]),
								(s.r[i]+randomi()*0.04)*sin(s.a[i]))); 
		}
		for(int i=0;i<m.N;i++){
				meas.points.push_back(TMeasurementPoint(
								(m.r[i]+randomi()*0.04)*cos(m.a[i]),
								(m.r[i]+randomi()*0.04)*sin(m.a[i]))); 
		}
		timer(true);
		matcher.match(ref,meas,80,matches);
		timer(false);
		fprintf(stderr,"N = %d, REF:(%.2f,%.2f,%.2f) MEAS:(%.2f,%.2f,%.2f)\n",matches.size(),
																																		ref_pos.x,ref_pos.y,ref_pos.a,
																																		meas_pos.x,meas_pos.y, meas_pos.a);
		
		for(int i=0;i<matches.size();i++){
				fprintf(stderr,"\ta=%6.3f\t w=%10.2f\t areal=%.3f\t err=%.2f\n",matches[i].angle,
																										matches[i].corrVal, ref_pos.a-meas_pos.a,
																							(matches[i].angle-(ref_pos.a-meas_pos.a))*180.0/M_PI);
		}
}

void TestHoughAngleWithData(const char *fname)
{
		float grid_size = 50.0; ///< size of the local grid
		float max_laser_dist = 10.0;
		int enough_scan_poinsts = 800; ///< enough to reserve memory for the scan points in the meas file.
		
		TOccupancyGrid O(grid_size,grid_size,0.1,Grid::TCoord2D(0,0));
		O.setGridToValue(0.5);
		
		Grid::TScan meas;
		meas.alloc(enough_scan_poinsts);
		Grid::pose pos;
		std::vector< Grid::pose > poses;    ///All poses for previous map
		
		
		int Nx,Ny;	
		unsigned char *rgb;
		O.getDim(Nx,Ny);
		rgb = (unsigned char *) malloc(3*Nx*Ny);
		addImageWindow(Nx, Ny);
		glutImageGUI_Start();
		
		FILE *log = fopen(fname,"rt"); ///< data file
		if(log == NULL){
				fprintf(stderr,"Unable to open the log file\n");
				exit(1);
		}
		int cnt = 0;
		
		CHoughAngleMatcher matcher(2500,1500);
		std::vector<TAngleMatch> matches;
		CEnvironmentMeasurement2D emeas,ref;
		
		Grid::pose diff_pose, prev_pose, pose_estimate, pose;
		
		while(!feof(log)){
				readScanLine(log,meas,pos);
				diff_pose.setToDifferentialPose(pos,prev_pose); ///movent in between scans
				prev_pose = pos;
				
				pose_estimate = pose.integrateDifferential(diff_pose); ///< Here we should be
				
				poses.push_back(pos);
				meas.filt(max_laser_dist);
				
				if(cnt==0){
						O.setScanFillProb(0, 0 ,0,meas,0.3,0.9);
				}else{
						ref.setOccupancyGrid(&O, 0.7);
						emeas.clear();
						for(int i=0;i<meas.N;i++){
								emeas.points.push_back(TMeasurementPoint( (meas.r[i]*cos(meas.a[i])+pose_estimate.x),
																					(meas.r[i]*sin(meas.a[i]))+pose_estimate.x)); 
								matcher.match(ref,emeas,sqrt(grid_size*grid_size+grid_size*grid_size),matches);
								timer(false);
						}
				}
				
				
				if(cnt%100==0){
						O.fillRGB(rgb,1.0/255.0);
						for(int i=0;i<poses.size();i++){
								Grid::TPixel pix = O.toPixel(Grid::TCoord2D(poses[i].x,poses[i].y));
								setRGBpix(rgb,pix.x,pix.y, Nx, 255,0, 0 );
						}
						displayImage(0, rgb,Ny,Nx);
				}
				cnt++;
		}
		
		
}


int main(int argc, char *argv[]){
		Grid::TCoord2D ocoord;
		Grid::TPixel pix;
		Grid::TScan s,m;
		int Nx,Ny;

		s.alloc(181);
		m.alloc(181);
		TOccupancyGrid *O;
		O = new TOccupancyGrid(1,1,0.04,Grid::TCoord2D(0,0));
		if(!O->loadLineMap(MAP_FILE_NAME, 1.0,1.0)){
				fprintf(stderr,"**** Unable to load the map\n");
				exit(1);
		}
		
		
		
		O->getDim(Nx,Ny);
		unsigned char *rgb = (unsigned char *) malloc(3*Nx*Ny);
		//addImageWindow(Nx, Ny);
		//glutImageGUI_Start();
		//setRGBmap(rgb,O, 220,220, 220);
		//displayImage(0, rgb,Ny,Nx);
		
		Grid::pose ref_pos(randomi()*10,randomi()*10,randomi()*M_PI);
		TestHoughAngle(rgb,O,ref_pos);
		


int cnt=0;
float r;

// 		while(1){
// 				setRGBmap(rgb,O, 240,240, 255);
// 				drawRobot(rgb, O, Grid::pose(0,0,M_PI/5), 255,0, 0 );
// 				drawRobot(rgb, O, Grid::pose(dx,dy,da), 0,255, 0 );
// 				drawRobot(rgb, O, Grid::pose(20,5,4.5), 0,0, 255 );
// 		
// 				displayImage(0, rgb,Ny,Nx);
// 				
// 		// 		if(cnt==0) {
// 		// 				saveJPEG("scans.jpg",rgb,Nx,Ny);
// 		// 				mcl::pose diff;
// 		// 				diff.setToDifferentialPose(mcl::pose(x_ref+dx,y_ref+dy,a_ref+da),mcl::pose(x_ref,y_ref,a_ref));
// 		// 				fprintf(stderr,"DIFF (%.3f, %.3f, %.3f)\n",diff.x,diff.y,diff.a);	
// 		// 
// 		// 
// 		// 				mcl::pose relative;
// 		// 				relative.set(0,0,0);
// 		// 				diff = relative.integrateDifferential(diff);
// 		// 
// 		// 				fprintf(stderr,"RELA (%.3f, %.3f, %.3f)\n",diff.x,diff.y,diff.a);
// 		// 
// 		// 				saveScan("refScan.txt",s);
// 		// 				saveScan("actScan.txt",m);
// 		// 		}
// 				cnt++;
// 				usleep(100*1000);
// 		}


return 0;
}